#include "serialport/serialport.h"
#include "armor_recog.hpp"
#include "angleSolver.hpp"
#include "camera.hpp"
#include "video.h"
#include <chrono>
#include <number_recog.hpp>
// #define CAMERA_PORT_DEBUG
using namespace chrono;
void angleSolverExample(void);
void dahuaCameraTest();
void demo();
void number_recog_test(bool isTrain);
using namespace std;

int main()
{
    demo();
    // string datasetPath = "etc/dataset/numbers.dat";
    // string modelPath = "etc/Number_KNN_model.mod";
    // printf("Please input the tag:");
    // int tag;
    // scanf("%d", &tag);
    // test(RED, tag, datasetPath);
    // number_recog_test(0);
    // demo();
    angleSolverExample();
    // dahuaCameraTest();
}

void demo()
{
    // angleSolverExample();
    // dahuaCameraTest();
    // Video video;
    Video *v = new Video();
    int num;
    Video::ETrigType type = Video::ETrigType::trigContinous;

    while (!(v->videoCheck()))
    {
        printf("please connect the camera\n");
        delete v;
        v = new Video();
    }

    while (!v->videoOpen())
    {
        delete v;
        v = new Video();
        printf("%d", num);
        num++;
        while (!v->videoCheck())
        {
            printf("%d  please connect the camera\n", num);
            num++;
            delete v;
            v = new Video();
        }
        printf("video open failed\n");
    }

    // while(!video.videoCheck()) {
    //     // v = &video;
    //     printf("please connect the camera!\n");
    // }
    // while (!video.videoOpen())
    // {
    //     printf("video open failed!\n");
    //     // video.videoClose();
    //     // v = new Video();
    // }
    v->setBalanceRatio(1, 1, 1);
    v->CameraChangeTrig(type);
    v->loadSetting(0);
    v->SetExposeTime(1000);
    if (!v->videoStart())
    {
        return;
    }
    v->setFrameRate(120);
    v->startGrabbing();
    Mat test1;
    Armor_recog ar = Armor_recog(RED, 0);
    angleSolver toAngle;
    VisionData vidata;
    SerialPort _port;
    toAngle.init(752, 480, 645.4, 55);
    _port.initSerialPort();

    // Mat testb;
    // VideoWriter writer("VideoTest.avi", VideoWriter::fourcc('M', 'J', 'P', 'G'), 25.0, Size(752, 480));
    while (1)
    {
        double t1 = (double)cv::getTickCount();
        vidata = {0, 0, 0, 0, 1, 0, 0};
        v->getFrame(test1);
        Point2f p(test1.cols / 2, test1.rows / 2);
        Mat rot = getRotationMatrix2D(p, 180, 1.0);
        warpAffine(test1, test1, rot, test1.size());
        // test1.copyTo(testb);
        if (test1.empty())
        {
            printf("Test1 is empty!\n");
            _port.TransformData(vidata);
            _port.send();
            // break;
        }
        else
        {
            ar.detect_armor(test1);

            if (ar.armor_detected.size() == 0)
            {
                printf("no armor detected!\n");
                _port.TransformData(vidata);
                _port.send();
            }
            else
            {
                toAngle.setEnemyPos(ar.armor_detected[0].center.x, ar.armor_detected[0].center.y, ar.armor_detected[0].barLen, 0, 0);
                toAngle.solve();
                double yaw = toAngle.getYaw();
                double pitch = toAngle.getPitch();
                float2uchar yaw_data, pitch_data;
                yaw_data.f = yaw;
                pitch_data.f = pitch;
                int yaw_temp = *(int *)&yaw_data.f;
                for (int i = 0; i < 4; i++)
                {
                    yaw_data.c[i] = yaw_temp >> (8 * i);
                }
                int pitch_temp = *(int *)&pitch_data.f;
                for (int i = 0; i < 4; i++)
                {
                    pitch_data.c[i] = pitch_temp >> (8 * i);
                }
                vidata.pitch_angle = pitch_data;
                vidata.yaw_angle = yaw_data;
                for (int i = 0; i < 4; i++)
                {
                    printf("%x", (int)yaw_data.c[i]);
                }
                printf("\n");
                for (int i = 0; i < 4; i++)
                {
                    printf("%x", (int)yaw_data.c[i]);
                }
                printf("yaw %f  pitch %f", yaw, pitch);
                double fps = 1 / (((double)cv::getTickCount() - t1) / cv::getTickFrequency());
                printf("fps:%f\n", fps);
                _port.TransformData(vidata);
                _port.send();
            }
        }
        // _port.closePort();   add to deinitializer, not necessary now
        waitKey(1);
#ifdef CAMERA_PORT_DEBUG
        delete v;
        printf("\nend it\n");
        exit(1);
#endif
    }
}

void angleSolverExample() //不考虑速度的版本
{
    angleSolver a = angleSolver();
    // a.test();
    a.init(752, 480, 645.4, 55); // initialize the size of camera capture, focal length of camera and real length of bar

    a.setBulletSpeed(25);                            // set speed of bullet
    a.setEnemyPos(376, 240, 10, M_PI / 8, M_PI / 6); // set position of enemy, length of bar in image and state of camera
    a.setEnemyVelocity(Point3f(3, 4, 0));            // set velocity of enemy
    if (a.solve() == -1)                             // 求解角度
    {
        cout << "not an accesible target" << endl;
        return;
    }
    double pitch = a.getPitch();
    double yaw = a.getYaw();
    unsigned char yaw_data[4];
    // sprintf((char*) yaw_data, "%.1f", yaw);
    // printf("%s", yaw_data);
    cout << "pitch: " << pitch << endl;
    cout << "yaw: " << yaw << endl;
}

void armorRecogExample()
{
    Camera camera;
    Mat test1;
    Armor_recog ar = Armor_recog(RED, 0);
    angleSolver toAngle;
    VisionData vidata;
    SerialPort _port;
    // Mat testb;
    // VideoWriter writer("VideoTest.avi", VideoWriter::fourcc('M', 'J', 'P', 'G'), 25.0, Size(752, 480));
    while (1)
    {
        double t1 = (double)cv::getTickCount();
        vidata = {0, 0, 0, 0, 1, 0, 0};
        camera.getImage(test1);
        Point2f p(test1.cols / 2, test1.rows / 2);
        Mat rot = getRotationMatrix2D(p, 180, 1.0);
        warpAffine(test1, test1, rot, test1.size());
        // test1.copyTo(testb);
        if (test1.empty())
        {
            cout << "Test1 is empty!" << endl;
            _port.initSerialPort();
            _port.TransformData(vidata);
            _port.send();
            // break;
        }
        else
        {
            //ar.imageProcess(test1);
            ar.detect_armor(test1);
            // for (size_t i = 0; i < ar.armor_detected.size(); i++)
            // {
            //     Point2f vertices[4];
            //     ar.armor_detected[i].bound.points(vertices);
            //     for (int j = 0; j < 4; j++)
            //     {
            //         line(testb, vertices[j], vertices[(j + 1) % 4], Scalar(255, 0, 0), 3);
            //         // imshow("test", test);
            //     }
            // }
            if (ar.armor_detected.size() == 0)
            {
                cout << "no armor detected!" << endl;
                _port.initSerialPort();
                _port.TransformData(vidata);
                _port.send();
            }
            else
            {
                toAngle.get_Position(ar.armor_detected.at(0));
                toAngle.setXYZDis(ar.armor_detected.at(0).position.x,
                                  ar.armor_detected.at(0).position.y,
                                  ar.armor_detected.at(0).position.z);

                //             //ar.clean();
                //             ar.armor_detected.clear();
                //             ar.edges_detected.clear();
                //             double a = toAngle.get_ALPHA();
                //             cout << toAngle.get_ALPHA() * 180 / M_PI  << " " << toAngle.get_THET() * 180 / M_PI << "  " << endl;

                //             vidata = {(float)toAngle.get_THET(), (float)toAngle.get_ALPHA(), (float)toAngle.getDis(), 0, 1, 0, 0};

                //             _port.initSerialPort();
                //             _port.TransformData(vidata);
                //             _port.send();
                //             //     // char* a = new char[9]{'h', 'e', 'l', 'l',  'o', '\0'};
                //             //     // write(_port.getFd(), a, 6);
                //             // }

                //         }

                double fps = 1 / (((double)cv::getTickCount() - t1) / cv::getTickFrequency());
                cout << "fps: " << fps << endl;
            }
            // imshow("TEST", testb);
            // writer.write(testb);

            // void angleSolverTest(){
            //     angleSolver a = angleSolver();
            //     a.test();
            // }
        }
        // writer.release();
        _port.closePort();
    }
}

/*
    dahua camera sample run
*/
void dahuaCameraTest()
{
    Video video;
    Mat frame;
    Video::ETrigType type = Video::ETrigType::trigContinous;

    while (!video.videoCheck())
    {
        printf("please connect the camera!\n");
    }
    if (!video.videoOpen())
    {
        printf("video open failed!\n");
    }
    video.setBalanceRatio(1, 1, 1);
    video.CameraChangeTrig(type);
    video.loadSetting(0);
    video.SetExposeTime(1000);
    if (!video.videoStart())
    {
        return;
    }
    video.setFrameRate(120);
    video.startGrabbing();

    for (;;)
    {
        auto start = system_clock::now();
        video.getFrame(frame);
        imshow("from camera", frame);
        waitKey(1);
        auto end = system_clock::now();
        auto duration = duration_cast<microseconds>(end - start);
        printf("seconds: %f\n", microseconds::period::den / (double(duration.count()) * microseconds::period::num));
    }
}

void number_recog_build_dataset()
{
    Number_recog nr;
}